import sys
sys.path.append('D:/Codes/tsp_robot')

import json
import numpy as np
from src.problem.tsptw.environment.tsptw import TSPTW

def GetNEUGraph():
    with open(file='D:/Codes/tsp_robot/src/problem/tsptw/solving/config.json', mode='r', encoding='utf-8') as f:
        str = f.read()
        points = json.loads(str)
    for point in points:
        point['x'] /= 20
        point['y'] /= 20
    n_city = len(points)
    x_coord = []
    y_coord = []
    for point in points:
        x_coord.append(point['x'])
        y_coord.append(point['y'])
    time_windows = np.zeros((n_city, 2))
    for i in time_windows:
        i[1] += 10000
    travel_time = []
    for i in range(n_city):
        dist = [(abs(x_coord[i] - x_coord[j]) + abs(y_coord[i] - y_coord[j]))
                for j in range(n_city)]
        travel_time.append(dist)
    print(travel_time)
    return TSPTW(n_city, travel_time, x_coord, y_coord, time_windows)

def GetTspGraph(tsp_path):
    travel_time = []
    x_coord = []
    y_coord = []
    with open(file=tsp_path, mode='r') as f:
        str = f.readlines()
    i = 0
    while i < len(str):
        line = str[i].split()
        if line[0] == 'DIMENSION':
            n_city = int(line[2])
            time_windows = np.zeros((n_city, 2))
        elif line[0] == 'EDGE_WEIGHT_SECTION':
            for j in range(n_city):
                i += 1
                travel_time.append(list(map(int, str[i].split())))
        elif line[0] == 'NODE_COORD_SECTION':
            for j in range(n_city):
                i += 1
                line = str[i].split()
                x_coord.append(int(line[1]))
                y_coord.append(int(line[2]))
        elif line[0] == 'TIME_WINDOW_SECTION':
            for j in range(n_city):
                i += 1
                line = str[i].split()
                time_windows[j][0], time_windows[j][1] = int(line[1]), int(line[2])
        elif line[0] == 'DEPOT_SECTION':
            break
        i += 1
    return TSPTW(n_city, travel_time, x_coord, y_coord, time_windows)


    
if __name__ == '__main__':
    GetTspGraph('D:/Codes/tsp_robot/src/problem/tsptw/solving/test20.tsptw')
